/***************************************************************************
 *  Filename: mainloopfunctions.cpp
 *  Class: MainLoopFunctions
 *  
 *  Copyright 2012  Jacco van der Spek
 *  <jaccovdspek@gmail.com>
 *
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *  
 ****************************************************************************/

/*
 *
 * This file is part of %ProjectName%.
 *
 *    %ProjectName% is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    %ProjectName% is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with %ProjectName%.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "mainloopfunctions.h"
#include <QTimer>
#include <QDebug>
#include <QFile>

MainLoopFunctions::MainLoopFunctions(BaroHandler *tbarometer, GPSHandler *tgpshandler, ParrotHandler *tparrothandler, GroundSCommunicator *txbee, VideoProcessing *tvideo, QObject *parent) :
    QObject(parent)
{
    barometer = tbarometer;
    gps = tgpshandler;
    parrot = tparrothandler;
    xbee = txbee;
    video = tvideo;

    connect(&timer, SIGNAL(timeout()), this, SLOT(run()));
    connect(xbee, SIGNAL(cmdFlyTo()), parrot, SLOT(sendTakeOff()));
    connect(xbee, SIGNAL(cmdReturn()), parrot, SLOT(sendLand()));
    connect(parrot, SIGNAL(receivedAltitude(int)), this, SLOT(updateParrotAltitude(int)));

    // Connections to take pictures.
    connect(&pictureTakeTimer, SIGNAL(timeout()), video, SLOT(takePictures()), Qt::DirectConnection);
    connect(video, SIGNAL(newFile(QString)), xbee, SLOT(sendImage(QString)));

    // Connections to send data
    connect(&senderTimer, SIGNAL(timeout()), SLOT(sendFlightData()));
    connect(&timer, SIGNAL(timeout()), SLOT(run()));
}

void MainLoopFunctions::run(){
    qDebug() << "[MainLoopFunctions] Spinning once";
    //Read GPS
    GPSData *tmpGPS = gps->getBufferedGPSData();
    if(tmpGPS){
        GPSData tmp = *tmpGPS;
        uavData.setGPSData(tmp);
        gpsTimeKeeper.restart();
        delete tmpGPS;
    }else{
        qWarning() << "[MainLoopFunctions] No GPS data received";
    }
    //Read Baro Altitude
    //Check for reading time-out.
    float alt = barometer->getAltitude();
    uavData.setBaroAltitude(alt);

    //Check for parrot time-out

    //Set Parameters for video processing
    video->setProperties(uavData.getAltitude(),  uavData.getPitch(), uavData.getVx(), uavData.getVy());
    //Get distances and steering
//    video->
    uavData.setSteering(video->getSteering());
    //Calculate target's location (gps coordinates)

    //Send commands to the parrot
    parrot->sendRelativeTarget(0, 0, 0);
}

void MainLoopFunctions::init(){
    barometer->setReference();
}

void MainLoopFunctions::updateParrotAltitude(int alt){
    float parAlt = (float)alt/100.0;
    uavData.setDroneAltitude(parAlt);
}

void MainLoopFunctions::updateParrotAngles(float roll, float pitch, float yaw){

}

void MainLoopFunctions::updateParrotBattery(int battery){

}

void MainLoopFunctions::updateParrotVelocities(float vx, float vy, float vz){

}

void MainLoopFunctions::startPictureTimer(int s){
    qDebug() << "[MainLoopFunctions] Starting pictures timer";
    pictureTakeTimer.start(s*1000);
}

void MainLoopFunctions::startSenderTimer(int ms){
    qDebug() << "[MainLoopFunctions] Starting sender timer";
    senderTimer.start(ms);
}

void MainLoopFunctions::sendFlightData(){
    xbee->sendFlightInfo(uavData);
    xbee->sendSteering(uavData.getSteering());
}

void MainLoopFunctions::startMainLoopTimer(int ms){
    timer.start(ms);
}
